#include "RigidBody.h"

namespace coolpool
{
	RigidBody::RigidBody(const RigidBody & rigid_body) :
		PointMass(rigid_body)
	{
		angular_momentum_ = rigid_body.angular_momentum_;
		coefficients_of_restitution_ = rigid_body.coefficients_of_restitution_;
		matrix_tools::copyMatrix(rigid_body.current_orientation_, current_orientation_);
		matrix_tools::copyMatrix(rigid_body.inertia_tensor_, inertia_tensor_);
		matrix_tools::copyMatrix(rigid_body.inertia_tensor_inverse_, inertia_tensor_inverse_);
		torque_ = rigid_body.torque_;
		std::copy(&rigid_body.status_rigid_body[0], &rigid_body.status_rigid_body[22], &status_rigid_body[0]);
	}

	RigidBody::RigidBody() :
		PointMass()
	{
		angular_momentum_ = math_tools::Vector3D(0.0, 0.0, 0.0);
		torque_ = math_tools::Vector3D(0.0, 0.0, 0.0);
		math_tools::matrix_tools::identityMatrix(current_orientation_);
		math_tools::matrix_tools::identityMatrix(inertia_tensor_);
		math_tools::matrix_tools::identityMatrix(inertia_tensor_inverse_);
	}

	RigidBody::~RigidBody()
	{

	}

	void RigidBody::clear()
	{
		PointMass::clear();
		torque_ = math_tools::Vector3D(0.0, 0.0, 0.0);
	}

	// Object is moved and rotated to the next position based on linear velocity
	// and angular velocity.
	// Again on the end the torque is reseted to be able "go back in time".
	void RigidBody::onUpdate(double time_step)
	{
		PointMass::onUpdate(time_step);

		saveRigidBody();
		angular_momentum_ += torque_;

		// Compute angular velocity.
		math_tools::Vector3D angular_velocity;
		matrix_tools::transform(inertia_tensor_inverse_, angular_momentum_, angular_velocity);

		if (angular_velocity.mag() < MINIMUM_FORCE)
		{
			angular_velocity = math_tools::Vector3D(0.0, 0.0, 0.0);
			angular_momentum_ = math_tools::Vector3D(0.0, 0.0, 0.0);
		}
		// If it is bigger than zero it can be applied to the body.
		if (angular_velocity.magSqr() > 0.0)
		{
			// Create the rotation matrix for the given time step.
			math_tools::Matrix3D rotation_matrix;
			// TODO: The * 3.0 is to make it look more real, probably the inertia tensor or something other need to be adjusted.
			//       Its no problem for spheres but would be for other objects.
			math_tools::matrix_tools::rotationMatrix(angular_velocity * 3.0, rotation_matrix);

			// Apply the rotation matrix to the current orientation.
			math_tools::matrix_tools::multiplyMatrix(rotation_matrix, current_orientation_, current_orientation_);
		}

		torque_ = math_tools::Vector3D(0.0, 0.0, 0.0);
	}

	void RigidBody::reverse()
	{
		PointMass::reverse();
		angular_momentum_ = math_tools::Vector3D(status_rigid_body[0], status_rigid_body[2], status_rigid_body[2]);
		torque_ = math_tools::Vector3D(status_rigid_body[3], status_rigid_body[4], status_rigid_body[5]);
		std::copy(&status_rigid_body[6], &status_rigid_body[22], &current_orientation_[0]);
	}

	math_tools::Vector3D RigidBody::pointVelocity(math_tools::Vector3D point)
	{
		return getLinearVelocity() + getAngularVelocity() * (point - getCenterOfMassLocation());
	}
}
